www.gusucode.com > Sphero Connectivity Package 程序工具箱matlab源码 > Sphero Connectivity Package/examples/sphero_simulink_examples.m
%% Sphero Simulink Library and Examples % This example describes the Simulink library for the Sphero Connectivity % package, and how the blocks from the library can be used to control a % Sphero. %% Sphero Simulink blocks % The Sphero Connectivity Package comes equipped with a Simulink library % containing basic sensing and actuation blocks. % % <matlab:open('sphero_lib') Open the library> open('sphero_lib'); %% % Specifically, the *Setup* block is needed to select a preexisting workspace % Sphero object for execution. That preexisiting Sphero object is then used % during the execution by the other blocks that specify the same name. % If you want to use different Spheros in the same execution then each % of them must have its own Setup block. % % The *Real Time Pacer* block can be used to slow down a fast execution % so it can track real time. % % The *Read Sensor* block calls the MATLAB "readSensor" function, which % returns the value of a specified sensor. % % The *Read Locator* block calls the MATLAB "readLocator" function, which % returns the current location and velocity components of the Sphero. % % The *Roll* block calls the MATLAB "roll" function to move the Sphero. % % The *Raw Motor* block calls the MATLAB "rawMotor" function to directly (and % independently) command the speed ot the two sphero motors (wheels). % % The bottom 4 blocks feature different mathematical models of the Sphero. % The *Kinematic models* approximate the sphero kinematic using an unicycle model. % The *Grey-Box models* also model the dynamics using a first order transfer function. % In both cases the inputs can be either desired speed and angle or left and right wheel speed. % % Also note that you can open the simulink example models (which will be shown % shortly thereafter), by double clicking on the three blocks on the right. close_system('sphero_lib'); %% Using the Read Sensors blocks to read acceleration % % <matlab:open('accel_sim') Open the example> open('accel_sim'); %% % This example uses three Read Sensor Roll blocks to sense acceleration % along the three body axis of the Sphero. % % The readSensor block can be used to sense any of the following signals: % 'accelX', 'accelY', 'accelZ', 'gyroX', 'gyroY', 'gyroZ','rmotorEmfRaw', % 'lmotorEmfRaw', 'lmotorPwmRaw', 'rmotorPwmRaw','imuPitch', 'imuRoll', % 'imuYaw', 'accelXFilt', 'accelYFilt','accelZFilt', 'gyroXFilt', % 'gyroYFilt', 'gyroZFilt','rmotorEmfFilt', 'lmotorEmfFilt', % 'Q0', 'Q1', 'Q2', 'Q3', 'distX','distY', 'accelOne', 'velX', 'velY' % % A sphero object must be created in the workspace before executing the % model % Create a Sphero object (if it does not exist) if ~exist('sph','var'), sph = sphero(); % Create a Sphero object end % make sure the object is connected connect(sph); % ping it result = ping(sph); % interrupt the example if ping was not successful if ~result, disp('Example aborted due to unsuccessful ping'); return, end % now we can actually execute the model: you can move the sphero around % to see the components of the acceleration along its 3 body axes changing sim('accel_sim'); % and close the system when the simulation is terminated close_system('accel_sim'); f1 = figure(1); plot(yout(:, 1), yout(:, 2:4)); grid title('Acceleration of Sphero along its X,Y and Z body axis') xlabel('time (sec)');ylabel('acceleration'); legend('accelX','accelY','accelZ'); %% Open-loop example using Roll block % % <matlab:open('roll_sim') Open the example> open('roll_sim'); %% % This example uses the Roll block to move the sphero, along a direction % specified by a sinusoid, with a constant speed of 70/255. This should % move the sphero along a path resembling an eight figure. % % The Read Locator block is used to gather the Sphero's position and velocity. % % A sphero object must be created in the workspace before starting % Create a Sphero object (if it does not exist) if ~exist('sph','var'), sph = sphero(); % Create a Sphero object end % make sure the object is connected connect(sph); % ping it result = ping(sph); % interrupt the example if ping was not successful if ~result, disp('Example aborted due to unsuccessful ping'); return, end % reset the calibration of the Sphero calibrate(sph, 0); % now we can actually execute the model sim('roll_sim'); % and close the system when the execution is terminated close_system('roll_sim'); f2 = figure(2); plot(yout(:, 2), yout(:, 3), '*') title({'x-y position of Sphero when running', 'open-loop example with Roll block'}) %% Open-loop example using the Raw Motor Sphero Kinematic Model % % <matlab:open('sph_control_sim') Open the example> open('rawmotor_sim_kine'); %% % This example uses a simple kinematic model of the Sphero (in which % separate power commands are given to the left and right motors) % instead of the Sphero itself. sim('rawmotor_sim_kine'); % and close the system when the execution is terminated close_system('rawmotor_sim_kine'); f3 = figure(3); plot(yout(:, 2), yout(:, 3), '*') title({'x-y position of Sphero when running', 'rawmotor kinematic model example'}) %% Closed-loop example using Roll block on the Kinematic model % % <matlab:open('sph_control_sim_kine') Open the example> open('sph_control_sim_kine'); %% % In this example, the "Outer Loop Control" block works as a controller. It % commands the desired velocity (speed and angle) of the sphero, in order % to minimize the difference between a reference point (which moves round a % square) and the measured position and velocity. % % The reference position and velocity are generated by the % "Reference" block, depending on a set of points (arranged in a square) % to be visited at a particular time. Both point positions and time are % parameters of the block. % % A simple kinematic model is used to double-check the control works fine. sim('sph_control_sim_kine'); % and close the system when the simulation is terminated close_system('sph_control_sim_kine'); f5 = figure(5); plot(yout(:, 2), yout(:, 3), '*') title({'x-y position of model when running', 'kinematic model control example'}) %% Closed-loop example using Roll block on the Grey Box model % % <matlab:open('sph_control_sim_grey') Open the example> open('sph_control_sim_grey'); %% % In this example, the "Outer Loop Control" block works as a controller. It % commands the desired velocity (speed and angle) of the sphero, in order % to minimize the difference between a reference point (which moves round a % square) and the measured position and velocity. % % The reference position and velocity are generated by the % "Reference" block, depending on a set of points (arranged in a square) % to be visited at a particular time. Both point positions and time are % parameters of the block. % % The grey-box model is used in place of the real sphero, to validate % the controller robustness. sim('sph_control_sim_grey'); % and close the system when the simulation is terminated close_system('sph_control_sim_grey'); f6 = figure(6); plot(yout(:, 2), yout(:, 3), '*') title({'x-y position of model when running', 'grey-box model control example'}) %% Closed-loop example using Roll block % % <matlab:open('sph_control_sim') Open the example> open('sph_control_sim'); %% % In this example, the "Outer Loop Control" block works as a controller. It % commands the desired velocity (speed and angle) of the sphero, in order % to minimize the difference between a reference point (which moves round a % square) and the measured position and velocity. % % The reference position and velocity are generated by the % "Reference" block, depending on a set of points (arranged in a square) % to be visited at a particular time. Both point positions and time are % parameters of the block. % % The sphero position and velocity are retrieved by the "Sphero Read % Locator" block. % ping the sphero result = ping(sph); % interrupt the example if ping was not successful if ~result, disp('Example aborted due to unsuccessful ping'); return, end % reset the calibration of the Sphero calibrate(sph, 0); % now we can actually execute sim('sph_control_sim'); % and close the system when the execution is terminated close_system('sph_control_sim'); f7 = figure(7); plot(yout(:, 2), yout(:, 3), '*') title({'x-y position of Sphero when running', 'closed loop control example'}); %% % Close all figures if required % close(f1); close(f2); close(f3); close(f4); % close(f5); close(f6); close(f7); close(f8); % Disconnect the Sphero disconnect(sph); %% See Also % <matlab:showdemo('sphero_examples') Sphero Connectivity Package Examples> %% % Copyright 2015-2017, The MathWorks, Inc.